#include <actionlib/client/simple_action_client.h> //actionlib客户端
/*
由于simple_action_client.h已经包含了ros/ros.h
可以省略
*/
#include "turtle_multipoints/turtleAction.h" //自定义action文件
#include <thread> //多线程头文件（c++11）需要在CMakeLists中添加
#include <iostream>
std::thread cancel_t;//声明一个线程对象
typedef actionlib::SimpleActionClient<turtle_multipoints::turtleAction> AC;//定义用到的action客户端的别名
actionlib::SimpleActionClient<turtle_multipoints::turtleAction> *ac;//创建action客户端对象指针

//用于取消正在进行的动作
void cancel(){
    std::string str;
    while (ros::ok()){
        std::cin >> str; //读取键盘值
        if("q"==str){
            ac->cancelGoal();//发送取消指令
            break;
        }
    }
}

// 判断动作是否完成，在任务结束后回调
void doneCb(const actionlib::SimpleClientGoalState& state, const turtle_multipoints::turtleResultConstPtr& result){
    std::string ret = state.toString();
    /*
    actionlib中事先定义了几种服务器完成动作的状态：
    中间状态：
            PENDING(待处理) - 目标尚未由动作服务器处理。
            ACTIVE(活动) - 目标正由动作服务器处理。
            RECALLING(重新调用中) - 目标尚未处理，并且已收到来自动作客户端的取消请求，但动作服务器尚未确认目标已被取消。
            PREEMPTING(抢占) - 目标正在处理，并且已从动作客户端接收到取消请求，但动作服务器尚未确认目标被取消。
    结束状态：
            REIECTED(已拒绝) - 目标被动作服务器拒绝，未经处理，没有来自动作客户端的取消请求。
            SUCCEEDED(成功) - 动作服务器成功完成了目标。
            ABORTED(中止) - 目标被动作服务器终止，而不是被动作客户端的外部请求取消。
            RECALLED(已调用) - 在动作服务器开始处理目标之前，目标已被新目标进行了替换、或被外部取消请求。取消
            PREEMPTD(抢占) - 目标的处理进程被另一个目标(替换)取消，或被发送到动作服务器的取消请求所取消。
    */
   if(ret == "SUCCEEDED"){
       ROS_INFO("task complete goal num = %d", result->goal_num);
   }else{
       ROS_INFO("Incomplete, -----Task response:%s ------",ret.c_str());
   }
    ros::shutdown();//与Ctrl+c功能相似
}

void feedbackCb(const turtle_multipoints::turtleFeedbackConstPtr& feedback){
    //任务进行中回调
    ROS_INFO("moving, current goal:(%f, %f)", feedback->feedback_x,feedback->feedback_y);
    ROS_INFO("current loop: %d", feedback->current_loop);
}

void activeCb(){
    //任务开始时回调
    ROS_INFO("Request received, start");
}

int main(int argc, char **argv){
    ros::init(argc, argv, "turtle_multipoints_client");
    ros::NodeHandle n("~");//局部命名空间，没有("~")为全局命名空间，因为launch文件中的param为局部param。
    ac = new AC("turtleAction",true); //定义客户端对象
    ROS_INFO("Waiting for action server to start.");
    ac->waitForServer();//一直等待turtleAction服务开启
    ROS_INFO("Action server started, sending goal.");
    turtle_multipoints::turtleGoal goal;

    int num,time;
    n.param<int>("point_num",num,5);
    n.param<int>("loop_time",time,10);
    std::cout<<"num:"<<num<<std::endl;
    std::cout<<"time:"<<time<<std::endl;
    goal.point_num = num;//请求服务端生成5个随机目标点
    goal.loop_time = time;//请求小乌龟循环运动10次
    ac->sendGoal(goal, &doneCb, &activeCb, &feedbackCb);
    /*
    doneCb(): 当服务端回调函数结束后，该函数被调用。
    activeCb(): 当服务端接收到请求后，开始动作时，该函数被调用。
    feedbackCb(): 服务端正在进行动作时，该函数被调用，实时反馈数据。
    注：如果不在意中间状态和结果，也可不写，直接发送请求：
            ac->sendGoal(goal);
    */
   cancel_t = std::thread(cancel);//创建并进入线程函数cancel()
   ros::spin();//循环调用回调函数
   if(cancel_t.joinable()){
       cancel_t.join();
    }
   delete ac;
   return 0;
}
